/*
 * ProtocolCan2Eth.cpp
 *
 *  Created on: Dec 8, 2011
 *      Author: kylintse
 */

#include <QtCore>

#include "klog.h"
#include "gdata.h"
#include "ProtocolCan2Eth.h"

#define CAN_LED_FLASH_TIME	100

///////////////////////////////////////////////////////////////////////////////
// Can2EthRecver
Can2EthRecver::Can2EthRecver(ProtocolCan2Eth* protocol,QObject* parent/*=0*/)
	: kthread(parent),m_protocol(protocol)
{

}

Can2EthRecver::~Can2EthRecver()
{

}

void Can2EthRecver::run()
{
	setStarted(true);

	while(true)
	{
		if(needExit())
			break;

		m_protocol->processRecv();
	}

	setStarted(false);
}

///////////////////////////////////////////////////////////////////////////////
// Can2EthSender
Can2EthSender::Can2EthSender(ProtocolCan2Eth* protocol,KjDeviceList* lstDevice,
		QObject* parent/*=0*/)
	: kthread(parent),m_protocol(protocol),m_lstDevice(lstDevice)
{

}

Can2EthSender::~Can2EthSender()
{

}

void Can2EthSender::run()
{
	setStarted(true);

	while(true)
	{
		if(needExit())
			break;

		m_protocol->processCommand();
	}

	setStarted(false);
}

///////////////////////////////////////////////////////////////////////////////
// ProtocolCan2Eth
ProtocolCan2Eth::ProtocolCan2Eth(quint32 id,const QString& devNode,KjDeviceList* lstDevice)
	: DeviceProtocol(id,devNode,lstDevice)
{
	m_curEthCount=0;
}

ProtocolCan2Eth::~ProtocolCan2Eth()
{

}
#ifndef PF_CAN
#define PF_CAN 29
#endif

#ifndef AF_CAN
#define AF_CAN PF_CAN
#endif
bool ProtocolCan2Eth::start()
{
	QString strCmd=QString("ifconfig %1 up").arg(m_devNode);
	if(::system(strCmd.toAscii().data())==-1)
	{
		glog->writeLog(llError,"ProtocolCan2Eth::start",
				QString("Bring %1 up failed").arg(m_devNode));
		return false;
	}

	m_sockfd=socket(PF_CAN,SOCK_RAW,CAN_RAW);

	if(m_sockfd==-1)
	{
		glog->writeLog(llError,"ProtocolCan2Eth::start",
				"Socket creation failed");
		return false;
	}

	struct ifreq ifr;
	strcpy(ifr.ifr_name,m_devNode.toAscii().data());
	ioctl(m_sockfd,SIOCGIFINDEX,&ifr);

	struct sockaddr_can addr;
	addr.can_family=AF_CAN;
	addr.can_ifindex=ifr.ifr_ifindex;
	bind(m_sockfd,(struct sockaddr*)&addr,sizeof(addr));

	m_recver=new Can2EthRecver(this);
	if(!m_recver->start(5000))
	{
		glog->writeLog(llError,"ProtocolCan2Eth::start",
				"Start can receive thread failed");
		::close(m_sockfd);
		return false;
	}

	QList<quint32> lstDev;
	for(int i=1;i<32;i++)
		lstDev<<i;

	quint32 ethIdCare;
	if(m_devNode=="can0")
		ethIdCare=0;
	else
		ethIdCare=1;

	m_lstDevice->registerCommandProcessor(m_id,lstDev,ethIdCare);

	m_sender=new Can2EthSender(this,m_lstDevice);
	if(!m_sender->start(5000))
	{
		glog->writeLog(llError,"ProtocolCan2Eth::start",
				"Start can send thread failed");
		m_recver->exit();
		delete m_recver;
		::close(m_sockfd);
		return false;
	}

	m_recycler=new DeviceProtocolRecycler(this);
	if(!m_recycler->start(5000))
	{
		glog->writeLog(llError,"ProtocolCan2Eth::start",
				"Start can recycle thread failed");
		m_recver->exit();
		delete m_recver;
		m_sender->exit();
		delete m_sender;
		::close(m_sockfd);
		return false;
	}

	return true;
}

void ProtocolCan2Eth::stop()
{
	::close(m_sockfd);

	m_recver->exit();
	m_recver->wait();
	delete m_recver;

	m_lstDevice->unRegisterCommandProcessor(m_id);
	m_sender->exit();
	m_sender->wait();
	delete m_sender;

	m_recycler->exit();
	m_recycler->wait();
	delete m_recycler;

	QMap<quint32,Can2EthFrame*>::const_iterator iframeAutoSend=m_framesAutoSend.constBegin();
	while(iframeAutoSend!=m_framesAutoSend.constEnd())
	{
		delete[] iframeAutoSend.value()->data;
		iframeAutoSend++;
	}
	qDeleteAll(m_framesAutoSend);
	m_framesAutoSend.clear();

	QMap<quint32,Can2EthFrame*>::const_iterator iframeInteract=m_framesInteract.constBegin();
	while(iframeInteract!=m_framesInteract.constEnd())
	{
		delete[] iframeInteract.value()->data;
		iframeInteract++;
	}
	qDeleteAll(m_framesInteract);
	m_framesInteract.clear();

	QMap<quint32,Can2EthCmdSource*>::const_iterator icmdSource=m_cmdSources.constBegin();
	while(icmdSource!=m_cmdSources.constEnd())
	{
		delete icmdSource.value();
		icmdSource++;
	}
	qDeleteAll(m_cmdSources);
	m_cmdSources.clear();
}

bool ProtocolCan2Eth::putCanFrameAutoSend(quint8 canFrameNum,quint32 ethCount,
		quint8 canAddr,char* data,quint8 dataLen)
{
	if(dataLen>8)
		return false;

	QMutexLocker lock(&m_lockFrameAutoSend);
	Can2EthFrame* frame;
	if(canFrameNum)
	{
		if(!m_framesAutoSend.contains(ethCount))
			return false;

		frame=m_framesAutoSend.value(ethCount);

		if(canFrameNum>=frame->canFrameWant)
			return false;

		memcpy(frame->data+8*canFrameNum-1,data,dataLen);
		frame->dataLen+=dataLen;
	}
	else
	{
		// First frame
		if(m_framesAutoSend.contains(ethCount))
		{
			qWarning()<<"Autosend frame ethcount same";
			delete[] m_framesAutoSend.value(ethCount)->data;
			delete m_framesAutoSend.value(ethCount);
			m_framesAutoSend.remove(ethCount);
		}

		if(!data[0])
			return false;

		quint32 dataBufLen=((quint8)(data[0]))*8-1;
		frame=new Can2EthFrame;
		frame->data=new char[dataBufLen];
		frame->canAddr=canAddr;
		frame->canFrameWant=data[0];
		frame->canFrameGet=0;
		frame->dataLen=dataLen-1;
		frame->timeEn=QDateTime::currentDateTime();
		memcpy(frame->data,data+1,dataLen-1);
		m_framesAutoSend.insert(ethCount,frame);
	}

	frame->canFrameGet++;
	if(frame->canFrameGet==frame->canFrameWant)
	{
		QByteArray dataFinish;
		if(m_devNode=="can0")
			dataFinish.append((char)0);
		else
			dataFinish.append(1);
		dataFinish.append(frame->data,frame->dataLen);
		m_lstDevice->putDatagram(dataFinish);
		delete[] frame->data;
		delete frame;
		m_framesAutoSend.remove(ethCount);
	}

	return true;
}

bool ProtocolCan2Eth::putCanFrameInteract(quint8 canFrameNum,quint32 ethCount,
		quint8 canAddr,char* data,quint8 dataLen)
{
	quint8 notifyFinish;
	if(ethCount&0x4000)
		notifyFinish=0;
	else
		notifyFinish=1;
	ethCount&=0x3ff;
	QMutexLocker lock(&m_lockFrameInteract);
	Can2EthFrame* frame;
	if(canFrameNum)
	{
		if(!m_framesInteract.contains(ethCount))
			return false;

		frame=m_framesInteract.value(ethCount);
		memcpy(frame->data+8*canFrameNum-1,data,dataLen);
		frame->dataLen+=dataLen;
	}
	else
	{
		// First frame
		if(!m_cmdSources.contains(ethCount))
			return false;

		if(m_framesInteract.contains(ethCount))
		{
			delete[] m_framesInteract.value(ethCount)->data;
			delete m_framesInteract.value(ethCount);
			m_framesInteract.remove(ethCount);
		}
		quint32 dataBufLen=data[0]*8-1;
		frame=new Can2EthFrame;
		frame->data=new char[dataBufLen];
		frame->canAddr=canAddr;
		frame->canFrameWant=data[0];
		frame->canFrameGet=0;
		frame->dataLen=dataLen-1;
		frame->timeEn=QDateTime::currentDateTime();
		memcpy(frame->data,data+1,dataLen-1);
		m_framesInteract.insert(ethCount,frame);
	}

	frame->canFrameGet++;
	if(frame->canFrameGet==frame->canFrameWant)
	{
//		qWarning()<<"Finish notify";
		Can2EthCmdSource* source=m_cmdSources.value(ethCount);

		m_lstDevice->putCommandNotify(source->source,source->devId,
				QByteArray(frame->data,frame->dataLen),notifyFinish);

		if(notifyFinish)
		{
			delete source;
			m_cmdSources.remove(ethCount);
		}
		delete[] frame->data;
		delete frame;
		m_framesInteract.remove(ethCount);
	}

	return true;
}

bool ProtocolCan2Eth::putCommandSource(quint32 ethCount,const QString& source,quint32 devId)
{
	QMutexLocker lock(&m_lockFrameInteract);
	if(m_cmdSources.contains(ethCount))
		return false;

	Can2EthCmdSource* cmdSource=new Can2EthCmdSource;
	cmdSource->source=source;
	cmdSource->devId=devId;
	cmdSource->timeEn=QDateTime::currentDateTime();
	m_cmdSources.insert(ethCount,cmdSource);
	return true;
}

void ProtocolCan2Eth::processRecv()
{
	fd_set readfd;
	struct timeval tv;
	qint32 ret;

	tv.tv_sec=1;
	tv.tv_usec=0;

	FD_ZERO(&readfd);
	FD_SET(m_sockfd,&readfd);
	ret=::select(m_sockfd+1,&readfd,NULL,NULL,&tv);

	switch(ret)
	{
		case -1:
		case 0:
			return;

		default:
		{
			if(m_devNode=="can0")
				gled->flashLed(led_Can[0],CAN_LED_FLASH_TIME);
			else
				gled->flashLed(led_Can[1],CAN_LED_FLASH_TIME);
			::read(m_sockfd,&m_curCanFrmRecv,sizeof(m_curCanFrmRecv));

			quint8 dataLen;
			quint8 autoSend;
			quint32 ethCount;
			quint8 canFrameNum;
			quint8 canAddr;

			dataLen=m_curCanFrmRecv.can_dlc;
			if(m_curCanFrmRecv.can_id&0x10000000)
			{
				autoSend=1;
			}
			else
				autoSend=0;
			ethCount=((m_curCanFrmRecv.can_id>>13)&0x43FF);
			canFrameNum=((m_curCanFrmRecv.can_id>>5)&0xFF);
			canAddr=(m_curCanFrmRecv.can_id&0x1F);

			if(autoSend)
			{
				putCanFrameAutoSend(canFrameNum,ethCount,canAddr,
						(char*)(m_curCanFrmRecv.data),dataLen);
			}
			else
			{
				putCanFrameInteract(canFrameNum,ethCount,canAddr,
						(char*)(m_curCanFrmRecv.data),dataLen);
			}

			break;
		}
	}
}

#define CAN2ETH_MAX_SEND			3
void ProtocolCan2Eth::processCommand()
{
	QByteArray data;
	QString source;
	quint32 serverType;
	quint32 devId;
	quint8 canFrameCount;
	quint32 restLen;
	char* pdata;
	quint32 curEthCount;
	int sendLen;
	int resendCount;
	bool sendOk=true;

	if(m_lstDevice->getCommand(m_id,source,serverType,devId,data,1000))
	{
		restLen=data.size();

		if((restLen+1)%8)
			canFrameCount=(restLen+1)/8+1;
		else
			canFrameCount=(restLen+1)/8;

		pdata=data.data();

		// First frame
		m_curCanFrmSend.can_dlc=(canFrameCount>1)?8:(restLen+1);
		curEthCount=m_curEthCount++;
		curEthCount=(curEthCount%0x3FF)+1;
		m_curCanFrmSend.can_id=((curEthCount<<13)|(1<<23)|
				devId|(serverType<<24)|CAN_EFF_FLAG);
		m_curCanFrmSend.data[0]=canFrameCount;
		memcpy(m_curCanFrmSend.data+1,pdata,(canFrameCount>1)?7:restLen);

		resendCount=0;
		if(m_devNode=="can0")
			gled->flashLed(led_Can[0],CAN_LED_FLASH_TIME);
		else
			gled->flashLed(led_Can[1],CAN_LED_FLASH_TIME);
		sendLen=::write(m_sockfd,&m_curCanFrmSend,sizeof(m_curCanFrmSend));
		while(sendLen!=sizeof(m_curCanFrmSend))
		{
			::usleep(100000);
			resendCount++;
			if(resendCount>=CAN2ETH_MAX_SEND)
			{
				sendOk=false;
				break;
			}
			sendLen=::write(m_sockfd,&m_curCanFrmSend,sizeof(m_curCanFrmSend));
		}

		restLen-=7;
		pdata+=7;
		for(int i=1;i<canFrameCount;i++)
		{
			m_curCanFrmSend.can_dlc=(restLen>8)?8:restLen;
			m_curCanFrmSend.can_id=((curEthCount<<13)|(1<<23)|
					(i<<5)|devId|(serverType<<24)|CAN_EFF_FLAG);
			memset(m_curCanFrmSend.data,0,8);
			memcpy(m_curCanFrmSend.data,pdata,m_curCanFrmSend.can_dlc);

			resendCount=0;
			if(m_devNode=="can0")
				gled->flashLed(led_Can[0],CAN_LED_FLASH_TIME);
			else
				gled->flashLed(led_Can[1],CAN_LED_FLASH_TIME);
			sendLen=::write(m_sockfd,&m_curCanFrmSend,sizeof(m_curCanFrmSend));
			while(sendLen!=sizeof(m_curCanFrmSend))
			{
				::usleep(100000);
				resendCount++;
				if(resendCount>=CAN2ETH_MAX_SEND)
				{
					sendOk=false;
					break;
				}
				sendLen=::write(m_sockfd,&m_curCanFrmSend,sizeof(m_curCanFrmSend));
			}

			restLen-=m_curCanFrmSend.can_dlc;
			pdata+=m_curCanFrmSend.can_dlc;
		}

		if(sendOk)
		{
			putCommandSource(curEthCount,source,devId);
		}
	}
	/*
	else
	{
		m_curCanFrmSend.can_dlc=8;
		curEthCount=1;
		m_curCanFrmSend.can_id=((curEthCount<<13)|(1<<23)|CAN_EFF_FLAG);
		m_curCanFrmSend.data[0]=0;
		m_curCanFrmSend.data[1]=1;
		m_curCanFrmSend.data[2]=2;
		m_curCanFrmSend.data[3]=3;
		m_curCanFrmSend.data[4]=4;
		m_curCanFrmSend.data[5]=5;
		m_curCanFrmSend.data[6]=6;
		m_curCanFrmSend.data[7]=7;

		if(m_devNode=="can0")
			gled->flashLed(led_Can[0],CAN_LED_FLASH_TIME);
		else
			gled->flashLed(led_Can[1],CAN_LED_FLASH_TIME);
		sendLen=::write(m_sockfd,&m_curCanFrmSend,sizeof(m_curCanFrmSend));
	}*/
}

void ProtocolCan2Eth::recycle()
{
	Can2EthFrame* frame;
	Can2EthCmdSource* cmdSource;
	quint32 ethCount;
	m_lockFrameAutoSend.lock();
	QMap<quint32,Can2EthFrame*>::const_iterator iframeAutoSend=m_framesAutoSend.constBegin();
	while(iframeAutoSend!=m_framesAutoSend.constEnd())
	{
		ethCount=iframeAutoSend.key();
		frame=iframeAutoSend.value();
		iframeAutoSend++;
		if(frame->timeEn.secsTo(QDateTime::currentDateTime())>20)
		{
			qWarning()<<"ProtocolCan2Eth::recycle():recycle autosend frame";
			delete[] frame->data;
			delete frame;
			m_framesAutoSend.remove(ethCount);
		}
	}
	m_lockFrameAutoSend.unlock();

	m_lockFrameInteract.lock();
	QMap<quint32,Can2EthFrame*>::const_iterator iframeInteract=m_framesInteract.constBegin();
	while(iframeInteract!=m_framesInteract.constEnd())
	{
		ethCount=iframeInteract.key();
		frame=iframeInteract.value();
		iframeInteract++;
		if(frame->timeEn.secsTo(QDateTime::currentDateTime())>120)
		{
			delete[] frame->data;
			delete frame;
			m_framesInteract.remove(ethCount);
		}
	}

	QMap<quint32,Can2EthCmdSource*>::const_iterator icmdSource=m_cmdSources.constBegin();
	while(icmdSource!=m_cmdSources.constEnd())
	{
		ethCount=icmdSource.key();
		cmdSource=icmdSource.value();
		icmdSource++;
		if(cmdSource->timeEn.secsTo(QDateTime::currentDateTime())>120)
		{
			delete cmdSource;
			m_cmdSources.remove(ethCount);
		}
	}
	m_lockFrameInteract.unlock();
}
